/*
 * @Description: NDT 匹配模块
 * @Author: Zhijian Qiao
 * @Date: 2020-02-08 21:46:57
 */
#ifndef avp_slam_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_
#define avp_slam_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_

#include <pcl/registration/ndt.h>
#include "registration_interface.hpp"

#ifdef USE_NDT_OMP
#include "pclomp/ndt_omp.h"
#include <pclomp/gicp_omp.h>
#endif

#include <omp.h>

namespace avp_slam {
    class NDTRegistration : public RegistrationInterface {
    public:

        NDTRegistration(const YAML::Node &node);

        NDTRegistration(float res, float step_size, float trans_eps, int max_iter);

        bool SetInputTarget(const CloudData::CLOUD_PTR &input_target) override;

//        eigen库对函数传参要求十分严格
        Eigen::Matrix4f ScanMatch(const CloudData::CLOUD_PTR &input_source,
                                  const Eigen::Matrix4f &predict_pose,
                                  CloudData::CLOUD_PTR &result_cloud_ptr) override;

        float GetFitnessScore() override;

        bool GetHasConverged() override;

    private:
        bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter);

    private:
#ifdef USE_NDT_OMP
        pclomp::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;
#else
        pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;
#endif

        float sumTime;

        uint32_t cnt;

        bool _hasConverged = false;

        float _fitnessScore = 10.f;

        float GetAverTime();

//        YAML::Node _node;
    };
}

#endif
